/*
 * MultiPIDController.cpp
 *
 *  Created on: Jan 15, 2011
 *      Author: user
 */


#include "MultiPIDController.h"
#include "Notifier.h"
#include "PIDSource.h"
#include "PIDOutput.h"
#include <math.h>
#include "Synchronized.h"



/**
 * Allocate a PID object with the given constants for P, I, D
 * @param Kp the proportional coefficient
 * @param Ki the integral coefficient
 * @param Kd the derivative coefficient
 * @param source The PIDSource object that is used to get values
 * @param output The PIDOutput object that is set to the output value
 * @param period the loop time for doing calculations. This particularly effects calculations of the
 * integral and differental terms. The default is 50ms.
 */
MultiPIDController::MultiPIDController(float Kp, float Ki, float Kd, float weight,
								PIDSource *source, PIDOutput *output,
								float period) :
	m_semaphore (0)
{
	m_semaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);

	m_controlLoop = new Notifier(MultiPIDController::CallCalculate, this);

	m_weightSum = 0;
	m_enabled = false;

	addPID( Kp, Ki, Kd, weight, source);


	
	m_pidOutput = output;
	m_period = period;

	m_controlLoop->StartPeriodic(m_period);
}

/**
 * Free the PID object
 */
MultiPIDController::~MultiPIDController()
{
	//
	semFlush(m_semaphore);
	delete m_controlLoop;
}

/*Adds an additional PID Calculation to the overall output
 *
 */
int MultiPIDController::addPID(float Kp, float Ki, float Kd, float weight, PIDSource *source){
	
	
	m_P.push_back(Kp);
	m_I.push_back(Ki);
	m_D.push_back(Kd);
	m_weight.push_back(weight);
	m_weightSum += weight;
	m_maximumOutput.push_back(1.0);
	m_minimumOutput.push_back(-1.0);

	m_maximumInput.push_back(0);
	m_minimumInput.push_back(0);

	m_continuous.push_back(false);
	m_setpoint.push_back(0);

	m_prevError.push_back(0);
	m_totalError.push_back(0);
	m_tolerance.push_back(0.05);

	m_result.push_back(0);

	m_pidInput.push_back(source);

	return m_pidInput.size()-1;
}

int MultiPIDController::removePID(int n){

	m_P.erase(m_P.begin() + n);
	m_I.erase(m_I.begin() + n);
	m_D.erase(m_D.begin() + n);
	m_weightSum -= m_weight[n];
	
	m_weight.erase(m_weight.begin() +n);
	
	m_maximumOutput.erase(m_maximumOutput.begin() + n);
	m_minimumOutput.erase(m_minimumOutput.begin() + n);

	m_maximumInput.erase(m_maximumInput.begin() + n);
	m_minimumInput.erase(m_minimumInput.begin() + n);

	m_continuous.erase(m_continuous.begin() + n);
	m_setpoint.erase(m_setpoint.begin() + n);

	m_prevError.erase(m_prevError.begin() + n);
	m_totalError.erase(m_totalError.begin() + n);
	m_tolerance.erase(m_tolerance.begin() + n);

	m_result.erase(m_result.begin() + n);

	m_pidInput.erase(m_pidInput.begin() + n);

	return m_pidInput.size()-1;
}


/**
 * Call the Calculate method as a non-static method. This avoids having to prepend
 * all local variables in that method with the class pointer. This way the "this"
 * pointer will be set up and class variables can be called more easily.
 * This method is static and called by the Notifier class.
 * @param controller the address of the PID controller object to use in the background loop
 */
void MultiPIDController::CallCalculate(void *controller)
{
	MultiPIDController *control = (MultiPIDController*) controller;
	control->Calculate();
}


/**
 * Read the inputs, calculate the outputs accordingly,scale, sum,  and write to the output.
 * This should only be called by the Notifier indirectly through CallCalculate
 * and is created during initialization.
 */
void MultiPIDController::Calculate()
{
	CRITICAL_REGION(m_semaphore)
	{
		
	m_finalResult = 0;
	for(unsigned int n=0; n < m_pidInput.size(); n++){
		if (m_pidInput[n] == 0)
			return;
		if (m_pidOutput == 0)
			return;
		float input = m_pidInput[n]->PIDGet();
		//printf("Sensor Value: %f , Setpoint: %f , ",input,m_setpoint[n] );

		if (m_enabled)
		{
			m_error[n] = m_setpoint[n] - input;
			if (m_continuous[n])
			{
				if (fabs(m_error[n]) >
					(m_maximumInput[n] - m_minimumInput[n]) / 2)
				{
					if (m_error[n] > 0)
						m_error[n] = m_error[n]  - m_maximumInput[n] + m_minimumInput[n];
					else
						m_error[n] = m_error[n]  +
						m_maximumInput[n] - m_minimumInput[n];
				}
			}

			if (((m_totalError[n] + m_error[n]) * m_I[n] < m_maximumOutput[n]) &&
					((m_totalError[n] + m_error[n]) * m_I[n] > m_minimumOutput[n]))
				m_totalError[n] += m_error[n];


			m_result[n] = m_P[n] * m_error[n] + m_I[n] * m_totalError[n] + m_D[n] * (m_error[n] - m_prevError[n]);
			m_prevError[n] = m_error[n];

			if (m_result[n] > m_maximumOutput[n])
				m_result[n] = m_maximumOutput[n];
			else if (m_result[n] < m_minimumOutput[n])
				m_result[n] = m_minimumOutput[n];

			m_finalResult += m_result[n]*m_weight[n]/m_weightSum;
		}

	}
	//printf("Output: %f \n",m_finalResult);
	m_pidOutput->PIDWrite(m_finalResult);
	}
	END_REGION;
}

/**
 * Set the PID Controller gain parameters.
 * Set the proportional, integral, and differential coefficients.
 * @param p Proportional coefficient
 * @param i Integral coefficient
 * @param d Differential coefficient
 */
void MultiPIDController::SetPID(float p, float i, float d, int n)
{
	CRITICAL_REGION(m_semaphore)
	{
		m_P[n] = p;
		m_I[n] = i;
		m_D[n] = d;
	}
	END_REGION;
}


/**
 * Get the Proportional coefficient
 * @return proportional coefficient
 */
float MultiPIDController::GetP(int n)
{
	CRITICAL_REGION(m_semaphore)
	{
		return m_P[n];
	}
	END_REGION;
}

/**
 * Get the Integral coefficient
 * @return integral coefficient
 */
float MultiPIDController::GetI(int n)
{
	CRITICAL_REGION(m_semaphore)
	{
		return m_I[n];
	}
	END_REGION;
}

/**
 * Get the Differential coefficient
 * @return differential coefficient
 */
float MultiPIDController::GetD(int n)
{
	CRITICAL_REGION(m_semaphore)
	{
		return m_D[n];
	}
	END_REGION;
}


/**
 * Return the current PID result
 * This is always centered on zero and constrained the the max and min outs
 * @return the latest calculated output
 */
float MultiPIDController::Get()
{
	float result;
	CRITICAL_REGION(m_semaphore)
	{
		result = m_finalResult;
	}
	END_REGION;
	return result;
}


/**
 *  Set the PID controller to consider the input to be continuous,
 *  Rather then using the max and min in as constraints, it considers them to
 *  be the same point and automatically calculates the shortest route to
 *  the setpoint.
 * @param continuous Set to true turns on continuous, false turns off continuous
 */
void MultiPIDController::SetContinuous(bool continuous, int n)
{
	CRITICAL_REGION(m_semaphore)
	{
		m_continuous[n] = continuous;
	}
	END_REGION;

}

/**
 * Sets the maximum and minimum values expected from the input.
 *
 * @param minimumInput the minimum value expected from the input
 * @param maximumInput the maximum value expected from the output
 */
void MultiPIDController::SetInputRange(float minimumInput, float maximumInput, int n)
{
	CRITICAL_REGION(m_semaphore)
	{
		m_minimumInput[n] = minimumInput;
		m_maximumInput[n] = maximumInput;
	}
	END_REGION;

	SetSetpoint(m_setpoint[n],n);
}


/**
 * Sets the minimum and maximum values to write.
 *
 * @param minimumOutput the minimum value to write to the output
 * @param maximumOutput the maximum value to write to the output
 */
void MultiPIDController::SetOutputRange(float minimumOutput, float maximumOutput, int n)
{
	CRITICAL_REGION(m_semaphore)
	{
		m_minimumOutput[n] = minimumOutput;
		m_maximumOutput[n] = maximumOutput;
	}
	END_REGION;
}


/**
 * Set the setpoint for the PIDController
 * @param setpoint the desired setpoint
 */
void MultiPIDController::SetSetpoint(float setpoint,int n)
{
	CRITICAL_REGION(m_semaphore)
	{
		if (m_maximumInput[n] > m_minimumInput[n]) {
			if (setpoint > m_maximumInput[n])
				m_setpoint[n] = m_maximumInput[n];
			else if (setpoint < m_minimumInput[n])
				m_setpoint[n] = m_minimumInput[n];
			else
				m_setpoint[n] = setpoint;
		}
		else
			m_setpoint[n] = setpoint;
	}
	END_REGION;
}


/**
 * Returns the current setpoint of the PIDController
 * @return the current setpoint
 */
float MultiPIDController::GetSetpoint(int n)
{
	float setpoint;
	CRITICAL_REGION(m_semaphore)
	{
		setpoint = m_setpoint[n];
	}
	END_REGION;
	return setpoint;
}



/**
 * Retruns the current difference of the input from the setpoint
 * @return the current error
 */
float MultiPIDController::GetError(int n)
{
	float  error;
	CRITICAL_REGION(m_semaphore)
	{
		error = m_error[n];
	}
	END_REGION;
	return error;
}

/*
 * Set the percentage error which is considered tolerable for use with
 * OnTarget.
 * @param percentage error which is tolerable
 */
void MultiPIDController::SetTolerance(float percent, int n)
{
	CRITICAL_REGION(m_semaphore)
	{
		m_tolerance[n] = percent;
	}
	END_REGION;
}


/*
 * Return true if the error is within the percentage of the total input range,
 * determined by SetTolerance. This asssumes that the maximum and minimum input
 * were set using SetInput.
 */
bool MultiPIDController::OnTarget()
{
	bool temp = true;
	CRITICAL_REGION(m_semaphore)
	{
		for(unsigned int n=0; n < m_pidInput.size(); n++){
			temp &= (fabs(m_error[n]) < m_tolerance[n] / 100 *
					(m_maximumInput[n] - m_minimumInput[n]));
		}
	}
	END_REGION;
	return temp;
}


/**
 * Begin running the PIDController
 */
void MultiPIDController::Enable()
{
	CRITICAL_REGION(m_semaphore)
	{

		m_enabled = true;
	}
	END_REGION;
}
/**
 * Stop running the PIDController, this sets the output to zero before stopping.
 */
void MultiPIDController::Disable()
{

	CRITICAL_REGION(m_semaphore)
	{
		m_pidOutput->PIDWrite(0);
		m_enabled = false;
	}
	END_REGION;
}


/**
 * Reset the previous error,, the integral term, and disable the controller.
 */
void MultiPIDController::Reset()
{
	Disable();

	CRITICAL_REGION(m_semaphore)
	{
		for(unsigned int n=0; n < m_pidInput.size(); n++){
			m_prevError[n] = 0;
			m_totalError[n] = 0;
			m_result[n] = 0;
		}
	}
	END_REGION;
}

